%Discrete PID control for continuous plant
clc, clear, close all

addpath('../../common');

ts=0.0025;%Sampling time

xk=zeros(2,1);

e_1 = 0;

u_1 = 0;

para_1 = 0;

duration = 1.0;

iteration = duration / ts;

inc = 0;

% allocate mem
time = zeros(1,iteration);
yd = zeros(1,iteration);
y = zeros(1,iteration);
e = zeros(1,iteration);
u = zeros(1,iteration);
para = zeros(1,iteration);

% generate noise
noise = 0.0*randn(1,iteration);

for k=1:1:iteration

    time(k)=k*ts;

    yd(k)=0.50*signal_generator(time(k));

    tSpan=[0 ts];

    [tt,xx]=ode45('iris_EOM' ,tSpan, xk,[],para_1);

    xk=xx(length(xx),:);

    y(k)=xk(1);

    e(k)=yd(k)-(y(k)+noise(k));

    de=(e(k)-e_1)/ts;
    
    inc = inc + e(k);
    
    u(k)=50.0*e(k)+0.3*de+ 0.5 *(inc);
    
    %Control limit
    if u(k) > 1.0
    u(k) = 1.0;
    end

    if u(k) < -1.0
    u(k) = -1.0;
    end
    
    % calculate rotor thrust
    para(k) = 5.84e-06 * 500 * u(k) *500 * abs(u(k));
    
    e_1=e(k);
    u_1= u(k);
    para_1 = para(k);
end

figure(1);

plot(time,yd,'r',time,y,'k', 'linewidth',2);

xlabel('time(s)');ylabel('yd,y');
legend('Tdeal position signal','Position tracking');
% 
figure(2);

plot(time,yd-y,'r','linewidth',2);

xlabel('time(s)'),ylabel('error');

figure(3);
plot(time,u,'r','linewidth',2);


